#include <iostream>
#include <algorithm>
#include <fstream>
#include <chrono>
#include <unistd.h>
#include <csignal>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include "System.h"

using namespace std;

// 全局SLAM系统指针，用于信号处理
ORB_SLAM2::System* gSLAM = nullptr;

// 信号处理函数
void signalHandler(int signum) {
    cout << "\n收到信号 " << signum << "，正在优雅退出..." << endl;
    if(gSLAM) {
        gSLAM->Shutdown();
        gSLAM = nullptr;
    }
    exit(signum);
}

int main(int argc, char **argv)
{
    // 注册信号处理器
    signal(SIGINT, signalHandler);
    signal(SIGTERM, signalHandler);

    if(argc < 3)
    {
        cerr << endl << "使用方法: " << argv[0] << " [词汇表文件] [配置文件] [左摄像头ID] [右摄像头ID(可选)]" << endl;
        cerr << "示例: " << argv[0] << " Vocabulary/ORBvoc.txt Examples/config/Stereo.yaml 0 1" << endl;
        return 1;
    }

    // 解析参数
    int left_camera_id = atoi(argv[3]);
    int right_camera_id = left_camera_id + 1; // 默认右摄像头ID为左摄像头ID+1
    if(argc >= 5) {
        right_camera_id = atoi(argv[4]);
    }

    cout << "==========================================" << endl;
    cout << "    CubeSLAM 双目实时SLAM启动" << endl;
    cout << "==========================================" << endl;
    cout << "左摄像头ID: " << left_camera_id << endl;
    cout << "右摄像头ID: " << right_camera_id << endl;
    cout << "配置文件: " << argv[2] << endl;
    cout << "词汇表: " << argv[1] << endl;

    // 检查文件是否存在
    ifstream vocab_file(argv[1]);
    if(!vocab_file.good()) {
        cerr << "错误: 词汇表文件不存在: " << argv[1] << endl;
        return 1;
    }
    vocab_file.close();

    ifstream config_file(argv[2]);
    if(!config_file.good()) {
        cerr << "错误: 配置文件不存在: " << argv[2] << endl;
        return 1;
    }
    config_file.close();

    // 尝试打开左摄像头
    cout << "\n正在测试左摄像头..." << endl;
    cv::VideoCapture cap_left;
    
    // 尝试不同的后端
    vector<int> backends = {cv::CAP_V4L2, cv::CAP_ANY, cv::CAP_GSTREAMER};
    vector<string> backend_names = {"V4L2", "ANY", "GStreamer"};
    
    bool left_camera_opened = false;
    for(size_t i = 0; i < backends.size(); i++) {
        cout << "尝试左摄像头后端: " << backend_names[i] << endl;
        cap_left.open(left_camera_id, backends[i]);
        if(cap_left.isOpened()) {
            cout << "左摄像头成功使用后端: " << backend_names[i] << endl;
            left_camera_opened = true;
            break;
        }
    }
    
    if(!left_camera_opened) {
        cerr << "错误: 无法打开左摄像头 " << left_camera_id << endl;
        return 1;
    }

    // 尝试打开右摄像头
    cout << "\n正在测试右摄像头..." << endl;
    cv::VideoCapture cap_right;
    
    bool right_camera_opened = false;
    for(size_t i = 0; i < backends.size(); i++) {
        cout << "尝试右摄像头后端: " << backend_names[i] << endl;
        cap_right.open(right_camera_id, backends[i]);
        if(cap_right.isOpened()) {
            cout << "右摄像头成功使用后端: " << backend_names[i] << endl;
            right_camera_opened = true;
            break;
        }
    }
    
    if(!right_camera_opened) {
        cerr << "错误: 无法打开右摄像头 " << right_camera_id << endl;
        cap_left.release();
        return 1;
    }

    // 设置摄像头属性
    cap_left.set(cv::CAP_PROP_FRAME_WIDTH, 640);
    cap_left.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
    cap_left.set(cv::CAP_PROP_FPS, 30);
    
    cap_right.set(cv::CAP_PROP_FRAME_WIDTH, 640);
    cap_right.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
    cap_right.set(cv::CAP_PROP_FPS, 30);

    // 测试摄像头读取
    cout << "测试双目摄像头读取..." << endl;
    cv::Mat test_left, test_right;
    int test_attempts = 0;
    const int max_test_attempts = 10;
    
    while(test_attempts < max_test_attempts) {
        cap_left >> test_left;
        cap_right >> test_right;
        if(!test_left.empty() && !test_right.empty()) {
            cout << "双目摄像头测试成功！" << endl;
            cout << "左摄像头帧尺寸: " << test_left.cols << "x" << test_left.rows << endl;
            cout << "右摄像头帧尺寸: " << test_right.cols << "x" << test_right.rows << endl;
            break;
        }
        test_attempts++;
        cout << "测试尝试 " << test_attempts << "/" << max_test_attempts << " 失败，重试..." << endl;
        usleep(100000); // 等待100ms
    }
    
    if(test_left.empty() || test_right.empty()) {
        cerr << "错误: 双目摄像头打开但无法读取有效帧" << endl;
        cap_left.release();
        cap_right.release();
        return 1;
    }

    // 创建SLAM系统
    cout << "\n创建双目SLAM系统..." << endl;
    ORB_SLAM2::System* SLAM = nullptr;
    try {
        SLAM = new ORB_SLAM2::System(argv[1], argv[2], ORB_SLAM2::System::STEREO, true);
        gSLAM = SLAM;
        cout << "双目SLAM系统创建成功！" << endl;
    } catch(const std::exception& e) {
        cerr << "错误: 双目SLAM系统创建失败: " << e.what() << endl;
        cap_left.release();
        cap_right.release();
        return 1;
    }

    cout << endl << "-------" << endl;
    cout << "开始双目实时SLAM..." << endl;
    cout << "按 Ctrl+C 优雅退出" << endl << endl;

    cv::Mat im_left, im_right;
    int frame_count = 0;
    int empty_frame_count = 0;
    const int max_empty_frames = 10;
    auto start_time = std::chrono::steady_clock::now();

    try {
        while(true) {
            // 捕获双目帧
            cap_left >> im_left;
            cap_right >> im_right;
            
            if(im_left.empty() || im_right.empty()) {
                empty_frame_count++;
                cerr << "警告: 第 " << frame_count << " 帧为空 (连续空帧: " << empty_frame_count << ")" << endl;
                
                if(empty_frame_count >= max_empty_frames) {
                    cerr << "错误: 连续 " << max_empty_frames << " 帧为空，退出程序" << endl;
                    break;
                }
                
                // 尝试重新打开摄像头
                if(empty_frame_count % 5 == 0) {
                    cout << "尝试重新打开双目摄像头..." << endl;
                    cap_left.release();
                    cap_right.release();
                    usleep(200000); // 等待200ms
                    cap_left.open(left_camera_id);
                    cap_right.open(right_camera_id);
                    if(cap_left.isOpened() && cap_right.isOpened()) {
                        cap_left.set(cv::CAP_PROP_FRAME_WIDTH, 640);
                        cap_left.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
                        cap_left.set(cv::CAP_PROP_FPS, 30);
                        cap_right.set(cv::CAP_PROP_FRAME_WIDTH, 640);
                        cap_right.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
                        cap_right.set(cv::CAP_PROP_FPS, 30);
                        cout << "双目摄像头重新打开成功" << endl;
                    } else {
                        cerr << "双目摄像头重新打开失败" << endl;
                    }
                }
                continue;
            }
            
            empty_frame_count = 0; // 重置空帧计数
            
            // 检查图像是否有效
            if(im_left.rows == 0 || im_left.cols == 0 || im_right.rows == 0 || im_right.cols == 0) {
                cerr << "警告: 图像尺寸无效" << endl;
                continue;
            }

            // 转换颜色空间
            cv::Mat im_left_gray, im_right_gray;
            try {
                if(im_left.channels() == 3) {
                    cv::cvtColor(im_left, im_left_gray, cv::COLOR_BGR2GRAY);
                } else if(im_left.channels() == 4) {
                    cv::cvtColor(im_left, im_left_gray, cv::COLOR_BGRA2GRAY);
                } else {
                    im_left_gray = im_left.clone();
                }
                
                if(im_right.channels() == 3) {
                    cv::cvtColor(im_right, im_right_gray, cv::COLOR_BGR2GRAY);
                } else if(im_right.channels() == 4) {
                    cv::cvtColor(im_right, im_right_gray, cv::COLOR_BGRA2GRAY);
                } else {
                    im_right_gray = im_right.clone();
                }
            } catch(const cv::Exception& e) {
                cerr << "颜色转换错误: " << e.what() << endl;
                continue;
            }

            // 处理双目SLAM
            try {
                auto timestamp = std::chrono::steady_clock::now();
                double time_sec = std::chrono::duration_cast<std::chrono::duration<double>>(timestamp - start_time).count();
                
                SLAM->TrackStereo(im_left_gray, im_right_gray, time_sec);
                
                frame_count++;
                if(frame_count % 30 == 0) {
                    cout << "已处理 " << frame_count << " 帧" << endl;
                }
                
            } catch(const std::exception& e) {
                cerr << "双目SLAM处理错误: " << e.what() << endl;
                continue;
            }

            // 检查退出条件
            char c = cv::waitKey(1);
            if(c == 'q' || c == 27) { // 'q' 或 ESC
                cout << "用户请求退出" << endl;
                break;
            }
        }
    } catch(const std::exception& e) {
        cerr << "主循环异常: " << e.what() << endl;
    }

    // 清理
    cout << "\n正在清理..." << endl;
    cap_left.release();
    cap_right.release();
    cv::destroyAllWindows();

    // 停止SLAM系统
    if(SLAM) {
        SLAM->Shutdown();
        delete SLAM;
        SLAM = nullptr;
        gSLAM = nullptr;
    }

    // 保存轨迹
    try {
        if(SLAM) {
            SLAM->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_stereo.txt");
            cout << "关键帧轨迹已保存到 KeyFrameTrajectory_stereo.txt" << endl;
        }
    } catch(const std::exception& e) {
        cerr << "保存轨迹时出错: " << e.what() << endl;
    }

    cout << "双目实时SLAM完成！" << endl;
    cout << "总处理帧数: " << frame_count << endl;
    
    auto final_time = std::chrono::steady_clock::now();
    double total_time = std::chrono::duration_cast<std::chrono::duration<double>>(final_time - start_time).count();
    if(total_time > 0) {
        cout << "平均FPS: " << frame_count / total_time << endl;
    }

    return 0;
}
